#include <MapIO.h>

#include <ros/ros.h>
#include <ros/package.h>

// #include <Json/JsonParser.hpp>


#include <boost/filesystem.hpp>
#include <boost/shared_ptr.hpp>

#include <opencv2/opencv.hpp>

#include <fstream>
#include <iostream>

const double MapIO::OCCUPIED_THRESH_ = 0.65;//pixel value:90
const double MapIO::FREE_THRESH_ = 0.196;//pixel value:205

std::string MapIO::mapDirPath_ = ros::package::getPath("kybot_map") + "/data";

MapIO::MapDataPtr MapIO::loadMap(std::string name)
{        
    MapIO::MapDataPtr mapDataPtr;

    boost::filesystem::path imageFilePath = boost::filesystem::path(mapDirPath_) / (name+".png");
    boost::filesystem::path mapMetaFilePath = boost::filesystem::path(mapDirPath_) / (name+".json");

    if(!boost::filesystem::is_regular_file(imageFilePath) || !boost::filesystem::is_regular_file(mapMetaFilePath))
    {
        // LOG(INFO)<<"Map does not exists: %s"<<name.data();
        return mapDataPtr;
    }

    mapDataPtr = boost::make_shared<MapIO::MapData>();

    // JsonParser object;
    using json = nlohmann::json;
    json object;
    std::ifstream ifile(mapMetaFilePath.string());
    ifile >> object;
    ifile.close();
    
    mapDataPtr->map.info.width = object["width"];
    mapDataPtr->map.info.height = object["height"];
    mapDataPtr->map.info.resolution = object["resolution"];
    mapDataPtr->initialPose.x = object["initial_pose"]["x"];
    mapDataPtr->initialPose.y = object["initial_pose"]["y"];
    mapDataPtr->initialPose.theta = object["initial_pose"]["theta"];
    mapDataPtr->map.info.map_load_time = ros::Time::now();
    mapDataPtr->map.info.origin.position.x = 0.0;
    mapDataPtr->map.info.origin.position.y = 0.0;
    mapDataPtr->map.info.origin.position.z = 0.0;
    mapDataPtr->map.info.origin.orientation.x = 0.0;
    mapDataPtr->map.info.origin.orientation.y = 0.0;
    mapDataPtr->map.info.origin.orientation.z = 0.0;
    mapDataPtr->map.info.origin.orientation.w = 1.0;
    
    mapDataPtr->virtualWalls.resize(object["virtual_walls"].size());
    
    for(int i=0; i<mapDataPtr->virtualWalls.size(); i++)
    {
        mapDataPtr->virtualWalls[i].points.resize(object["virtual_walls"][i].size());
        
        for(int j=0; j<mapDataPtr->virtualWalls[i].points.size(); j++)
        {
            mapDataPtr->virtualWalls[i].points[j].x = object["virtual_walls"][i][j]["x"];
            mapDataPtr->virtualWalls[i].points[j].y = object["virtual_walls"][i][j]["y"];
        }
    }

    cv::Mat image = cv::imread(imageFilePath.string(), cv::IMREAD_GRAYSCALE);
    mapDataPtr->map.info.height = image.rows;
    mapDataPtr->map.info.width = image.cols;
    int height= mapDataPtr->map.info.height, width = mapDataPtr->map.info.width;
    mapDataPtr->map.data.resize(height*width);
    for (size_t i = 0; i < height; i++)
    {
        for (size_t j = 0; j < width; j++)
        {
            double occ = (255 - image.at<uchar>(height-1-i, j)) / 255.0;//opencv以图片左上角为原点,而occupangrid以图片左下角为原点,因此需要颠倒
            if(occ > OCCUPIED_THRESH_)
            {
                mapDataPtr->map.data[i*width+j] = 100;
            }
            else if(occ < FREE_THRESH_)
            {
                mapDataPtr->map.data[i*width+j] = 0;
            }
            else 
            {
                mapDataPtr->map.data[i*width+j] = -1;
            }
        }                
    }
    mapDataPtr->map.info.map_load_time = ros::Time::now();
    mapDataPtr->map.header.frame_id = "map";
    mapDataPtr->map.header.stamp = ros::Time::now();

    return mapDataPtr;
}

std::string MapIO::getDefaultMapName()
{
    boost::filesystem::path mapListFilePath = boost::filesystem::path(mapDirPath_) / "map_list.json";
    // JsonParser object;
    using json = nlohmann::json;
    json object;
    std::ifstream ifile(mapListFilePath.string());
    ifile >> object;
    ifile.close();

    return object["default_map"].get<std::string>();
}

MapMetaDataPtr MapIO::loadMapMetaData(std::string name)
{
    MapMetaDataPtr ptr;

    boost::filesystem::path mapMetaDataFilePath = boost::filesystem::path(mapDirPath_) / (name+".json");
    if(!boost::filesystem::exists(mapMetaDataFilePath.string()))
    {
        // LOG(INFO)<<"File does not exists: %s"<<mapMetaDataFilePath.string().data();
        return ptr;
    }

    // JsonParser object;
    using json = nlohmann::json;
    json object;
    std::ifstream file(mapMetaDataFilePath.string());
    try
    {
        file >> object;
    }
    catch (...)
    {
        // LOG(INFO)<<"JSON file format invalid!";
        return ptr;
    }
    file.close();
    
    ptr = MapMetaDataPtr(new MapMetaData());
    try
    {            
        ptr->width = object["width"];
        ptr->height = object["height"];
        ptr->resolution = object["resolution"];
        ptr->initialPose.x = object["initial_pose"]["x"];
        ptr->initialPose.y = object["initial_pose"]["y"];
        ptr->initialPose.theta = object["initial_pose"]["theta"];
        ptr->chargeStationPose.x = object["charge_station_pose"]["x"];
        ptr->chargeStationPose.y = object["charge_station_pose"]["y"];
        ptr->chargeStationPose.theta = object["charge_station_pose"]["theta"];


        ptr->virtualWalls.resize(object["virtual_walls"].size());
        for(int i=0; i<ptr->virtualWalls.size(); i++)
        {
            ptr->virtualWalls[i].points.resize(object["virtual_walls"][i].size());

            for(int j=0; j<ptr->virtualWalls[i].points.size(); j++)
            {
                ptr->virtualWalls[i].points[j].x = object["virtual_walls"][i][j]["x"];
                ptr->virtualWalls[i].points[j].y = object["virtual_walls"][i][j]["y"];
            }
        }   
    }
    catch(...)
    {
        ROS_WARN("JSON file format invalid!");
        return ptr;
    }
    
    return ptr;
}

void MapIO::saveMapMetaData(std::string name, MapMetaDataPtr metaData)
{
    boost::filesystem::path mapMetaDataFilePath = boost::filesystem::path(mapDirPath_) / (name+".json");

    // JsonParser object;
    using json = nlohmann::json;
    json object;
    std::ofstream file(mapMetaDataFilePath.string());        
    object["width"] = metaData->width;
    object["height"] = metaData->height;
    object["resolution"] = metaData->resolution;
    object["initial_pose"]["x"] = metaData->initialPose.x;
    object["initial_pose"]["y"] = metaData->initialPose.y;
    object["initial_pose"]["theta"] = metaData->initialPose.theta;
    object["charge_station_pose"]["x"] = metaData->chargeStationPose.x;
    object["charge_station_pose"]["y"] = metaData->chargeStationPose.y;
    object["charge_station_pose"]["theta"] = metaData->chargeStationPose.theta;

    if(metaData->virtualWalls.empty())
    {
        // object["virtual_walls"]= JsonParser::array();
    }
    else
    {
        for(int i=0; i<metaData->virtualWalls.size(); i++)
        {
            for(int j=0; j<metaData->virtualWalls[i].points.size(); j++)
            {
                object["virtual_walls"][i][j]["x"] = metaData->virtualWalls[i].points[j].x;
                object["virtual_walls"][i][j]["y"] = metaData->virtualWalls[i].points[j].y;
            }
        }
    }
    

    file << object.dump(4);
    file.close();
}